

function [rpy,order] = my_tr2rpy(R, varargin)
        
    opt.deg = true;
    opt.order = {'zyx', 'xyz', 'arm', 'vehicle', 'yxz', 'camera'};
    opt = tb_optparse(opt, varargin);

    rpy = zeros(1,3);   
    opt.order = 'zyx';
    % % old ZYX order (as per Paul book)
    % if abs(abs(R(3,1)) - 1) < eps  % when |R31| == 1
    %     % singularity

    %     rpy(1) = 0;     % roll is zero
    %     if R(3,1) < 0
    %         rpy(3) = -atan2(R(1,2), R(1,3));  % R-Y
    %     else
    %         rpy(3) = atan2(-R(1,2), -R(1,3));  % R+Y
    %     end
    %     rpy(2) = -asin(R(3,1));
    % else
    %     rpy(1) = atan2(R(3,2), R(3,3));  % R
    %     rpy(3) = atan2(R(2,1), R(1,1));  % Y
             
    %     [~,k] = max(abs( [R(1,1) R(2,1) R(3,2) R(3,3)] ));
    %     switch k
    %         case 1
    %             rpy(2) = -atan(R(3,1)*cos(rpy(3))/R(1,1));
    %         case 2
    %             rpy(2) = -atan(R(3,1)*sin(rpy(3))/R(2,1));
    %         case 3
    %             rpy(2) = -atan(R(3,1)*sin(rpy(1))/R(3,2));
    %         case 4
    %             rpy(2) = -atan(R(3,1)*cos(rpy(1))/R(3,3));
    %     end
    % end
    rpy(3) = atan2(R(2,1), R(1,1));  % R
    rpy(2) = atan2(-R(3,1), sqrt(R(3,2)^2+R(3,3)^2));  % P
    rpy(1) = atan2(R(3,2), R(3,3));  % Y


    if opt.deg
        rpy = rpy * 180/pi;
    end
    if nargout > 1
        order = opt.order;
    end
end